'''
Created on Aug 17, 2010

@author: Tom Smith
'''

import serial
import RobotData
import time

class Robot(object):
    '''
    classdocs
    '''
    com_port = ''
    ser = ''
    rd = RobotData.RobotData()
    BIN1 = (2300,5000,0)
    BIN2 = (26000,5000,0)
    RETREAT = (26000,100,0)
    SCANNER = (15500,5000,0)
    SCANNER_LEVEL = (15500,5000,12000)
    BIN1_LEVEL = (2300,5000,12000)
    height_cutoff_ratio = .50
    z_down_increment = 100
    last_bin_1 = ''
    scanner_height = ''
    last_material_height = ''

    def __init__(self, com_port):
        '''
        Constructor
        '''
        self.com_port = com_port
        self.ser = serial.Serial(com_port, 115200)
        
    def getData(self):
        self.sendCommand('D', True)
    def getSensorData(self):
        self.sendCommand('i', True)
        self.sendCommand('i', True)

    def setNextPosition(self, position):
        x = position[0]
        y = position[1]
        z = position[2]
        self.sendCommand(str(x) + 'P', True)
        self.sendCommand(str(y) + 'Q', True)
        self.sendCommand(str(z) + 'R', True)
    
    def gotoInternalBin1(self):
        self.sendCommand('h', True)
    
    def gotoBin1(self):
        self.setNextPosition(self.BIN1)
        self.gotoNextPosition()
    
    def gotoInternalBin2(self):
        self.sendCommand('k', True)
        
    def gotoBin2(self):
        self.setNextPosition(self.BIN2)
        self.gotoNextPosition()
    
    def gotoInternalRetreat(self):
        self.sendCommand('j', True)
        
    def gotoRetreat(self):
        self.setNextPosition(self.RETREAT)
        self.gotoNextPosition()
    
    def openScanner(self):
        self.sendCommand('o', True)
        
    def closeScanner(self):
        self.sendCommand('c', True)
    
    def toggleVacuum(self):
        self.sendCommand('v', True)
        
    def lowerAndLiftInternal(self):
        self.sendCommand('l', True)
        
    def lowerAndLift(self):
        self.getSensorData()
        self.getData()
       # print "Ratio: %s" % self.rd.ratio
        x = self.rd.x_position
        y = self.rd.y_position
        z = self.rd.z_position
        file = open('ratio_rec_light.csv', 'wb')
        file.write("z_position, ratio, photo, infrared\n")
        while z < 12000:
            z = int(z) + 200
            next_position = (x, y, z)
            #print next_position
            self.setNextPosition(next_position)
            self.gotoNextPosition()
            self.getSensorData()
            file.write("%s,%s, %s, %s\n" % (z, self.rd.ratio, self.rd.photo, self.rd.infrared))
            print "Ratio: %s" % self.rd.ratio
        print "At material height"
        file.close()
            
        
    def gotoInternalCenter(self):
        self.sendCommand('g', True)
        
    def gotoScanner(self):
        self.setNextPosition(self.SCANNER)
        self.gotoNextPosition()

    def gotoScannerLevel(self):
        self.setNextPosition(self.SCANNER_LEVEL)
        self.gotoNextPosition()
        
    def gotoBin1Level(self):
        self.setNextPosition(self.BIN1_LEVEL)
        self.gotoNextPosition()
        
    def gotoNextPosition(self):
        self.sendCommand('S', True)
        
    def moveY(self, direction):
        if direction == 'f':
            self.sendCommand('w', True)
        elif direction == 'b':
            self.sendCommand('f', True)
        else:
            raise SyntaxError('Direction should be either f(forward) or b(back)')
        
    def zeroTheMachine(self):
        self.sendCommand('M', True)  
         
    def sendCommand(self, command, read):
        '''Sends a command to the robot'''
        self.ser.write(command)
        ser_in = ''
        #Wait until the robot has completed the command
        while(ser_in == ""):
            ser_in = self.ser.readline()
            #print ser_in
            if ser_in[0] == "<":
                self.rd.sortOutXML(ser_in)
                print "X: %s Y: %s Z: %s" % (self.rd.x_position, self.rd.y_position, self.rd.z_position)
        
        